#include "EigenTools.h"

namespace coolpool
{
	double distanceFromCenterSqr(const Eigen::VectorXd & vec)
	{
		Eigen::VectorXd center(vec.rows());
		setZero(&center);
		return distanceSqr(center, vec);
	}

	double distanceSqr(const Eigen::VectorXd & vec1, const Eigen::VectorXd & vec2)
	{
		assert(vec1.rows() == vec2.rows());
		double distSqr = 0.0;
		for (int i = 0; i < vec1.rows(); ++i)
		{
			distSqr += math_tools::sqr(vec2[i] - vec1[i]);
		}
		return distSqr;
	}

	double distance(const Eigen::VectorXd & vec1, const Eigen::VectorXd & vec2)
	{
		return sqrt(distanceSqr(vec1, vec2));
	}

	Eigen::VectorXd * sliceVector(const Eigen::VectorXd & vector, std::set<int> s)
	{
		Eigen::VectorXd * out = new Eigen::VectorXd(s.size());
		int i = 0;
		for (std::set<int>::iterator row = s.begin(); row != s.end(); ++row)
		{
			(*out)[i++] = vector[*row];
		}

		return out;
	}

	Eigen::MatrixXd * sliceMatrix(const Eigen::MatrixXd matrix, std::set<int> s)
	{
		Eigen::MatrixXd * out = new Eigen::MatrixXd(s.size(), s.size());
		int i = 0;
		int j = 0;
		for (std::set<int>::iterator row = s.begin(); row != s.end(); ++row)
		{
			for (std::set<int>::iterator col = s.begin(); col != s.end(); ++col)
			{
				(*out)(i, j++) = matrix(*row, *col);
			}
			j = 0;
			i++;
		}

		return out;
	}

	void setZero(Eigen::VectorXd * vector)
	{
		for (int i = 0; i < vector->rows(); ++i)
		{
			(*vector)[i] = 0;
		}
	}

	void setZero(Eigen::MatrixXd * matrix)
	{
		for (int i = 0; i < matrix->rows(); ++i)
		{
			for (int j = 0; j < matrix->cols(); ++j)
			{
				(*matrix)(i, j) = 0;
			}
		}
	}
}
